In recent years, as a robot technology is developed, robots are executing complicated tasks which have been performed thus far by humans. Examples of the complicated tasks include assembly of industrial products. To autonomously execute an assembling operation, a robot grasps a component using an end effector such as a hand. To grasp a component using a robot hand, a relative position and a relative posture between the component to be grasped and the robot are to be measured. Such measurement of a position and a posture is required to be performed in various cases, in addition to assembly of industrial products performed by a robot, such as estimation of a position and a posture for self position identification and re-configuration of a 3D space performed by an autonomous mobile robot.
Examples of a method for measuring a position and a posture include a method using model fitting. In the model fitting, 3D-shaped models of an object are assigned to geometric features detected from monochrome images or color images captured by a camera and assigned to distance images obtained by individually measuring the images using a distance sensor. The monochrome images and the color images described above are collectively referred to as 2D images hereinafter. In model fitting performed on a 2D image, geometric features of a 3D-shaped model are projected on the image so as to obtain correspondences with geometric features detected in the image. Then, a position and a posture are repeatedly corrected so that distances in the individual correspondences are minimized. In this way, the relationship between a position and a posture of the camera and a position and a posture of the object is calculated. In model fitting performed on a distance image, measurement points in the distance image are converted into 3D points having 3D coordinates, and correspondences between the 3D points obtained by the conversion and 3D points on a 3D-shaped model are obtained.
Then, a position and a posture are repeatedly corrected so that distances in the individual correspondences are minimized. In this way, a position-and-posture relationship between a camera and an object is calculated.
In a measurement method utilizing the model fitting, it is important to accurately obtain correspondences between geometric features of a 3D-shaped model and geometric features of a captured image. In the method described above, approximate values of a position and a posture are input as initial values of the position and the posture, and the correspondences between the geometric features of the 3D-shaped model and the geometric features of the captured image are obtained in accordance with the approximate values of the position and the posture. In this method, when the approximate values of the position and the posture are shifted from true values of the position and the posture, generation of failure correspondences is not avoidable. When a number of failure correspondences are generated, a process of estimating the position and the posture fails and accuracy of estimation of the position and the posture is deteriorated.
To address this problem, a method for reducing influence of failure correspondences by using M estimation for correspondences between geometric features of a 3D-shaped model and geometric features of a captured image has been discussed (refer to NPL 1, for example).
In this method, when a distance of a correspondence is near an average value, a large weight is applied whereas when a distance of a correspondence is far from the average value, a small weight is applied. In this way, it is estimated that a correspondence point far from the average value has a low correspondence reliability so that influence on calculation of a position and a posture is minimized.
However, in the method disclosed in Non-Patent Literature 1, degrees of contributions of the individual correspondences to a result of a process of calculating the position and the posture are controlled in accordance with the distances between the geometric features of the 3D-shaped model and the geometric features of the captured image in the correspondences. Specifically, the smaller a distance in a correspondence is, the larger a degree of a contribution sets. However, this method is merely used for excepting outliers in numerical calculation, and the determination is not performed on the individual correspondences taking geometric similarities based on an object shape such as positions of the geometric features of the 3D-shaped model and the geometric features of the captured image in the object and shapes of the geometric features of the 3D-shaped model and the geometric features of the captured image into consideration. Therefore, when a failure correspondence is generated but a distance between a geometric feature of the 3D-shaped model and a geometric feature of the captured image in the correspondence incidentally becomes small due to the shift of approximate values of the position and the posture from true values of the position and the posture, it is difficult to remove adverse effect caused by the failure correspondence using the M-estimation.